/*
 * Detector.cpp
 *
 *  Created on: Apr 20, 2010
 *      Author: Soheil Damangir
 */

#include <iostream>
#include <stdio.h>
#include "image.h"
#include "log.h"
#include "printfstring.h"

#include "Detector.h"
#include "../Util/Conf.h"
#include "../Util/ImageLog.h"
#include "time_tools.h"

using kthrobot::Log;
using namespace kthrobot;
namespace Group5{

Detector::Detector(std::istream&is): classifier(is), last_t(0)
{
  clr = new bool*[Constants::get().IMG_Y];
  clr[0] = new bool[Constants::get().IMG_X*Constants::get().IMG_Y];
  for (int i = 1; i < Constants::get().IMG_Y; i++)
    clr[i] = clr[0] + i*Constants::get().IMG_X;
  for (int x = 0; x < Constants::get().IMG_X; x++) {
    for (int y = 0; y < Constants::get().IMG_Y; y++) {
      clr[y][x] = false;
    }
  }
  sw.Start();
}

void Detector::detect(kthrobot::Image&image, DetectionResults&results, const char* what="")
{
  if (sw.Micros32() - last_t < 1e6) {
    return;
  }
  else last_t = sw.Micros32();
  uint8_t*pixels = image.buffer();
  int pxl[3];
  int x=0,y=0;
  int min_x = Constants::get().IMG_X, max_x = 0, min_y = Constants::get().IMG_Y, max_y = 0;
  int DX = 0;
  for (y = 0; y < Constants::get().IMG_Y; y++) {
    for (x = 0; x < Constants::get().IMG_X; x++) {
      if (x==0)
      {
        DX = 0;
      }
      pxl[0]=(int)*(pixels);pxl[1]=(int)*(pixels+1);pxl[2]=(int)*(pixels+2);
      bool tmp = classifier.classify(pxl) > 0;

      clr[y][x] = tmp;

      if (clr[y][x]){
        DX++;

        if(x > max_x)
          max_x = x;
        else if(x < min_x)
          min_x = x;

        if(y > max_y)
          max_y = y;
        else if(y < min_y)
          min_y = y;

      }else{
        if (DX < x && DX < Constants::get().MORPH_DX)
        {
          for (int xx = x-DX;xx < x; xx++)
            clr[y][xx]=false;
        }
        DX = 0;

      }
      pixels += image.num_components();
    }
  }
  results.size = 0;
  results.x = 0;
  results.y = 0;

  if (min_x < 1) min_x = 1;
  if (min_y < 1) min_y = 1;
  if (max_x > Constants::get().IMG_X-2) max_x = Constants::get().IMG_X-2;
  if (max_y > Constants::get().IMG_Y-2) max_y = Constants::get().IMG_Y-2;

  _max_x = min_x, _max_y = min_y, _min_x = min_x, _min_y = min_y;
  for (y = min_y;y <= max_y; y++)
  {
    for (x = min_x;x <= max_x; x++)
    {
      bool left=clr[y-1][x];
      bool right = clr[y+1][x];
      bool self = clr[y][x];
      clr[y][x] = self&&(left||right);
      if (clr[y][x])
      {
        results.size++;
        results.x += x;
        results.y += y;
        if(x > _max_x)
          _max_x = x;
        else if(x < _min_x)
          _min_x = x;

        if(y > _max_y)
          _max_y = y;
        else if(y < _min_y)
          _min_y = y;
      }
    }
  }
  if (results.size > 0)
  {
    results.x /= results.size;
    results.y /= results.size;
  }
  results.detected = results.size > Constants::get().MIN_BALL_SIZE;
  results.h = _max_y - _min_y;
  results.w = _max_x - _min_x;
  if (strcmp("", what) != 0)
  {
    for (int i = _min_y;i<_max_y;i++){clr[y][_min_x] = true;clr[y][_max_x] = true;}
    for (int i = _min_x;i<_max_x;i++){clr[_min_y][x] = true;clr[_max_y][x] = true;}
    ImageLog::get().LogBinary(clr,Constants::get().IMG_X,Constants::get().IMG_Y,PrintFString("%s_%dx%d_at_%d_%d",what, results.w, results.h, results.x, results.y).c_str());
  }
}
Detector::~Detector()
{
  delete[] clr[0];
  delete[] clr;
}

}
